#include "can_speaker.h"

CanSpeaker::CanSpeaker(const std::shared_ptr<rclcpp::Node>& node)
    : node_(node),
    bEnable_(false), 
    silentAudio_{0, 0},
    runAudio_{0, 0}, 
    alarmAudio_ {0, 0},
    obstacleAudio_{0, 0},
    spinAudio_{0, 0},
    currentMode_{SOUND_SILENT},
    previousMode_{SOUND_SILENT}
{  
}

CanSpeaker::~CanSpeaker() {
    running_.store(false);
    if (workerThread_.joinable()) {
        workerThread_.join();
    }
}

void CanSpeaker::readParameters() {
    // Load parameters
    std::string deviceType;
    node_->get_parameter("canSpeaker/type", deviceType);

    // Configure audio based on device type
    if (deviceType == "canopen") {
        silentAudio_ = {0xff, 0};
        DEBUG_STREAM_OUT("Using CANopen speaker configuration");
    } else if (deviceType == "can") {
        silentAudio_ = {0, 0};
        DEBUG_STREAM_OUT("Using CAN speaker configuration");
    }

    // Load audio parameters
    node_->get_parameter("canSpeaker/enable", bEnable_);
    node_->get_parameter("canSpeaker/runAudio", runAudio_.first);
    node_->get_parameter("canSpeaker/runVolume", runAudio_.second);
    node_->get_parameter("canSpeaker/alarmAudio", alarmAudio_.first);
    node_->get_parameter("canSpeaker/alarmVolume", alarmAudio_.second);
    node_->get_parameter("canSpeaker/obstacleAudio", obstacleAudio_.first);
    node_->get_parameter("canSpeaker/obstacleVolume", obstacleAudio_.second);
    node_->get_parameter("canSpeaker/spinAudio", spinAudio_.first);
    node_->get_parameter("canSpeaker/spinVolume", spinAudio_.second);
}

void CanSpeaker::init() {
     // Initialize parameters
    readParameters();
    client_ = node_->create_client<agv_srvs::srv::ControlVoice>(
        "/agv_control_voice");
    
    // Start worker thread
    running_.store(true);
    workerThread_ = std::thread(&CanSpeaker::workerThread, this);

    DEBUG_STREAM_OUT("CanSpeaker initialized. Enable state: " <<  
               (bEnable_ ? "true" : "false")); 
}

void CanSpeaker::workerThread() {
    while (running_.load()) {
        if (enable()) {
            audioOut();
        }
        std::this_thread::sleep_for(50ms);
    }
}

void CanSpeaker::setAudioMode(SoundMode mode) {
    std::lock_guard<std::mutex> lock(stateMutex_);
    currentMode_ = mode;
}

void CanSpeaker::audioOut() {
    std::lock_guard<std::mutex> lock(stateMutex_);
    
    if (currentMode_ == previousMode_) return;
    previousMode_ = currentMode_;

    if (!client_->service_is_ready()) {  // ROS2内置非阻塞检查
        DEBUG_ERROR_OUT("Voice service not available");
        return;
    }
    
    const auto start = node_->now();
    try {
        auto request = std::make_shared<agv_srvs::srv::ControlVoice::Request>();
        
        switch(currentMode_) {
            case SOUND_SILENT:
                request->audio = silentAudio_.first;
                request->volume = silentAudio_.second;
                break;
            case SOUND_RUN:
                request->audio = runAudio_.first;
                request->volume = runAudio_.second;
                break;
            case SOUND_ALARM:
                request->audio = alarmAudio_.first;
                request->volume = alarmAudio_.second;
                break;
            case SOUND_OBSTACLE:
                request->audio = obstacleAudio_.first;
                request->volume = obstacleAudio_.second;
                break;
            case SOUND_SPIN:
                request->audio = spinAudio_.first;
                request->volume = spinAudio_.second;
                break;
            default:
                DEBUG_WARN_OUT("Unknown sound mode: " << currentMode_);
                return;
        }

        auto result = client_->async_send_request(request);
        
        // Wait for response with timeout
        if (rclcpp::spin_until_future_complete(
            node_->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) 
        {
            auto response = result.get();
            if (response->success) {
                DEBUG_OUT("Audio set successfully in %.2fms", 
                            (node_->now() - start).seconds() * 1000);
            } else {
                DEBUG_WARN_OUT("Service call failed with error:" << response->success);
            }
        } else {
            DEBUG_ERROR_OUT("Service call timed out");
        }
    } catch (const std::exception& e) {
        DEBUG_ERROR_OUT("Service call exception: " << e.what());
    }
}